/*NNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNN

	Copyright (c) 2009 Jounayd Id Salah.
	All rights reserved.

	This file is part of Blacklib.

	Blacklib is free software: you can redistribute it and/or modify it under the 
	terms of the GNU Lesser General Public License as published by the Free Software 
	Foundation, either version 3 of the License, or (at your option) any later version.
	Blacklib is distributed in the hope that it will be useful, but WITHOUT ANY WARRANTY; 
	without even the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR 
	PURPOSE.  See the GNU Lesser General Public License for more details.
	You should have received a copy of the GNU Lesser General Public License along with 
	Blacklib.  If not, see <http://www.gnu.org/licenses/>.
*/
//NNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNN

#include "maths/valid.h"
#include "maths/angleaxis.h"
#include <assert.h>

namespace black
{
	//***************************************************************
	// quat
	//***************************************************************

	//__________________________________________________________________________
#ifdef bDEBUG
	quat::quat() : x( F_NAN ), y( F_NAN ), z( F_NAN ), w( F_NAN ) {}
#else
	quat::quat() {}
#endif

	//__________________________________________________________________________
	quat::quat( const float _x, const float _y, const float _z, const float _w ) : x( _x ), y( _y ), z( _z ), w( _w )
	{
		assert( isValid() );
	}

	//__________________________________________________________________________
	quat::quat( const quat& q ) : x( q.x ), y( q.y ), z( q.z ), w( q.w )
	{
		assert( isValid() );
	}

	//__________________________________________________________________________
	quat::quat( const float fVals[] ) : x( fVals[0] ), y( fVals[1] ), z( fVals[2] ), w( fVals[3] )
	{
		assert( isValid() );
	}

	//__________________________________________________________________________
	quat::quat( const vec3& vFrom, const vec3& vTo )
	{
		assert( vFrom.isValid() );
		assert( vTo.isValid() );
		const vec3 vCross = vFrom.cross( vTo );
		assert( !vCross.isZero() );
		x = vCross.x;
		y = vCross.y;
		z = vCross.z;
		w = vFrom.dot( vTo );
		normalize();
		assert( isValid() );
	}

	//__________________________________________________________________________
	quat::quat( const angleAxis& aa )
	{
		assert( aa.isValid() );

		x = aa.axis.x;
		y = aa.axis.y;
		z = aa.axis.z;

		const float fHalfAngle = aa.angle * .5f;

		w = cos( fHalfAngle );
		const float fSinHalf = sin( fHalfAngle );
		x = x * fSinHalf;
		y = y * fSinHalf;
		z = z * fSinHalf;
	}

	//__________________________________________________________________________
	float quat::operator[] ( const size_t i ) const
	{
		assert( i < 4 );
		return *(&x+i);
	}

	//__________________________________________________________________________
	float& quat::operator[] ( const size_t i )
	{
		assert( i < 4 );
		return *(&x+i);
	}

	//__________________________________________________________________________
	quat& quat::operator+= ( const quat& q )
	{
		x += q.x;
		y += q.y;
		z += q.z;
		w += q.w;

		return *this;
	}

	//__________________________________________________________________________
	quat& quat::operator-= ( const quat& q )
	{
		x -= q.x;
		y -= q.y;
		z -= q.z;
		w -= q.w;

		return *this;
	}

	//__________________________________________________________________________
	quat& quat::operator*= ( const quat& q )
	{
		quat qTmp;
		qTmp.w = w*q.w - x*q.x - y*q.y - z*q.z;
		qTmp.x = w*q.x - z*q.y + x*q.w + y*q.z;
		qTmp.y = w*q.y - x*q.z + y*q.w + z*q.x;
		qTmp.z = w*q.z + z*q.w + x*q.y - y*q.x;

		*this = qTmp;

		return *this;
	}

	//__________________________________________________________________________
	quat& quat::operator*= ( const float f )
	{
		x *= f;
		y *= f;
		z *= f;
		w *= f;

		return *this;
	}

	//__________________________________________________________________________
	quat& quat::operator/= ( const float f )
	{
		const float fInv = 1.f / f;
		x *= fInv;
		y *= fInv;
		z *= fInv;
		w *= fInv;

		return *this;
	}

	//__________________________________________________________________________
	quat quat::operator* ( const quat& q ) const
	{
		return quat(	w*q.x + q.w*x + y*q.z - q.y*z,
						w*q.y + q.w*y + z*q.x - q.z*x,
						w*q.z + q.w*z + x*q.y - q.x*y,
						w*q.w - x*q.x - y*q.y - z*q.z );
	}

	//__________________________________________________________________________
	quat quat::operator* ( const float f ) const
	{
		return quat( x*f, y*f, z*f, w*f );
	}

	//__________________________________________________________________________
	quat quat::operator/ ( const float f ) const
	{
		assert( f != 0.f );
		const float fInv = 1.f / f;
		return quat( x*fInv, y*fInv, z*fInv, w*fInv );
	}

	//__________________________________________________________________________
	quat quat::operator+ ( const quat& q ) const
	{
		return quat( x+q.x, y+q.y, z+q.z, w+q.w );
	}

	//__________________________________________________________________________
	quat quat::operator- ( const quat& q ) const
	{
		return quat( x-q.x, y-q.y, z-q.z, w-q.w );
	}

	//__________________________________________________________________________
	void quat::conjugate()
	{
		x = -x;
		y = -y;
		z = -z;
	}

	//__________________________________________________________________________
	float quat::length() const
	{
		return sqrt( x*x + y*y + z*z + w*w );
	}

	//__________________________________________________________________________
	float quat::squaredLength() const
	{
		return x*x + y*y + z*z + w*w;
	}

	//__________________________________________________________________________
	quat& quat::normalize()
	{
		const float fLength = sqrt( x*x + y*y + z*z + w*w );
		assert( fLength != 0.f );
		const float fInvLength = 1.f / fLength;
		x *= fInvLength;
		y *= fInvLength;
		z *= fInvLength;
		w *= fInvLength;

		return *this;
	}

	//__________________________________________________________________________
	void quat::setMult( const quat& q1, const quat& q2 )
	{
		x = q1.w*q2.w - q1.x*q2.x - q1.y*q2.y - q1.z*q2.z;
		y = q1.w*q2.x + q2.w*q1.x + q1.y*q2.z - q2.y*q1.z;
		z = q1.w*q2.y + q2.w*q1.y + q1.z*q2.x - q2.z*q1.x;
		w = q1.w*q2.z + q2.w*q1.z + q1.x*q2.y - q2.x*q1.y;
	}

	//__________________________________________________________________________
	vec3 quat::rot( const vec3& v ) const
	{
		const vec3 qv( x, y, z );

		const float	fDot = qv.dot( v );
		const vec3	vCross( qv.cross( v ) );
		const vec3 vRes = ( v*( w*w-0.5f ) + vCross*w + qv*fDot );
		return vRes + vRes;
	}

	//__________________________________________________________________________
	float quat::dot( const quat& q ) const
	{
		return x*q.x + y*q.y + z*q.z + w*q.w;
	}

	//__________________________________________________________________________
	vec3 quat::invRot( const vec3& v ) const
	{
		const vec3 qv( x, y, z );

		const float	fDot = qv.dot( v );
		const vec3	vCross( qv.cross( v ) );
		const vec3 vRes = ( v*( w*w-0.5f ) - vCross*w + qv*fDot );
		return vRes + vRes;
	}

	//__________________________________________________________________________
	bool quat::isUnit() const
	{
		return ( ( x*x + y*y + z*z + w*w ) - 1.f ) < fEPSILON;
	}

	//__________________________________________________________________________
	bool quat::isValid() const
	{
		return black::isValid( x ) && black::isValid( y ) && black::isValid( z ) && black::isValid( w );
	}

	//__________________________________________________________________________
	quat& quat::nlerp( const quat& q1, const quat& q2, float t )
	{
		if ( q1.dot( q2 ) >= 0.f )
		{
			*this = q1 + (q2 - q1) * t;
		}
		else
		{
			*this = q1 - (q2 + q1) * t;
		}

		return normalize();
	}

	//***************************************************************
	// quat_128
	//***************************************************************

	//__________________________________________________________________________
	quat_128::quat_128() {}

	//__________________________________________________________________________
	quat_128::quat_128( const float _x, const float _y, const float _z, const float _w ) : quat( _x, _y, _z, _w ) {}

	//__________________________________________________________________________
	quat_128::quat_128( const quat& q ) : quat( q ) {}

	//__________________________________________________________________________
	quat_128::quat_128( const quat_128& q ) : quat( q ) {}

	//__________________________________________________________________________
	quat_128::quat_128( const float fVals[] ) : quat( fVals ) {}

	//__________________________________________________________________________
	quat_128::operator __m128&() { return reinterpret_cast<__m128&>( *this ); }

	//__________________________________________________________________________
	quat_128::operator const __m128&() const { return reinterpret_cast<const __m128&>( *this ); }

}
